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ABSTRACT 


Energy  storage  is  very  limited  in  AUV’s.  To  assist  with  energy  management,  data 
gathering  missions  have  been  proposed  where  the  vehicle  should  sit  on  the  bottom  and  gather 
acoustic/video/chemical  data  over  extended  periods  of  time.  In  this  grounding  senario  while 
thrusters  may  be  used,  they  are  less  desirable  because  of  their  high  energy  consuption  and 
restricted  use  close  to  the  ocean  floor.  The  purpose  of  this  work  is  to  study  a  low  cost,  simple  soft 
grounding  capability  for  a  submersible  vehicle  using  controllable  ballast.  The  ballast  system 
based  on  the  NFS  Phoenix  AUV  is  designed  to  control  weight  addition  into  or  out  of  two  ballast 
tanks.  The  developed  control  law  adjusts  the  pump  flow  rate  keeping  the  pitch  angle  and  depth 
rate  within  the  limits.  Results  for  a  soft  grounding  operation  have  been  obtained  using  simulation. 
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I. 


INTRODUCTION 


A.  INTRODUCTION 

Energy  storage  is  very  limited  in  AUV’s.  To  assist  with  energy  management,  data  gathering 
missions  have  been  proposed  where  the  vehicle  should  sit  on  the  bottom  and  gather 
acoustic/video/chemical  data  over  extended  periods  of  time.  In  this  grounding  senario,  thrusters 
may  be  used.  However,  there  are  two  disadvantages  for  this  method:  high  energy  consumption  and 
restricted  use  close  to  the  ocean  bottom.  The  motivation  for  this  thesis  is  to  study  a  low  cost,  simple 
soft  grounding  capability  for  a  submersible  vehicle  using  controllable  ballast.  For  simplicity,  water 
ballast  is  considered.  The  design  of  the  control  system  is  based  on  the  NFS  Phoenix  AUV.  The 
ballast  system  is  designed  to  control  the  weight  addition  into  or  out  of  the  two  ballast  tanks  where 
instead  of  a  bang-bang  or  a  fuzzy  logic  controller  which  actually  turns  on  and  turns  off  the  pumps  , 
the  developed  control  law  controls  the  pump  flow  rate,  keep  the  pitch  angle  and  depth  rate  within 
limits. 

B.  BACKGROUND 

Ballast  control  of  vehicles  is  not  a  new  subject  and  we  can  find  many  examples  begiiming 
in  the  1900’s,  the  non-rigid  airships  are  very  good  examples  of  ballast  control.  One  of  the  most 
important  elements  of  a  non-rigid  airship  is  the  ballonet-system.  A  ballonet  as  seen  in  Figure  1-1  is 
an  airbag  (one  or  two  of  them)  inside  the  envelope,  which  is  provided  with  air  from  a  blower  or 
directly  fi'om  the  engine  unit.  The  air  could  be  removed  from  the  ballonet  through  the  valves.  If  the 
airship  has  a  fi’ont  and  aft  ballonet  then  the  height  of  the  airship  can  be  steered.  For  example,  if  the 
aft  ballonet  is  filled  with  more  air,  then  the  airship  will  become  heavier  in  the  rear  part  of  the 
envelope  and  the  ship  will  incline  increasing  the  altitude  of  the  ship  by  using  the  engines.  As  Figure 
1-2  depicted,  the  airship  can  also  be  trimed  through  a  front  and  aft  ballonet  [Ref  1]. 
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For  most  of  the  maneuvering  underwater  vehicles,  the  depth  control  is  normally  provided 
by  hydroplanes.  As  an  example,  consider  the  NFS  Phoenix  AUV.  It  has  two  sets  of  control 
surfaces,  namely  the  bow  and  the  stem  planes.  At  low  speed,  the  control  surfeces  cannot  provide 
enough  control  authority  and  the  ballast  control  problem  is  very  complex  due  to  nonlinear,  time- 
varying,  uncertain  hydrodynamics.  There  are  some  designs  that  used  bang-bang  control  system 
[Ref  2].  The  ARPA’s  Unmanned  Undersea  Vehicle  (UUV)  employs  a  fuzzy  logic  ballast 
controller  which  is  comparable  with  the  performance  that  can  be  obtained  from  standard  control 
techniques,  but  does  not  require  traditional  linear  or  nonlinear  design  methods. 


In  another  fuzzy  logic  control  model,  a  15  state  Kalman  filter  was  developed  to  provide 
estimates  of  the  motion  variables  and  the  applied  lift  and  torque  acting  on  the  UUV.  The  control 
law  decides  between  three  possible  control  actions;  pump  water  in  both  tanks,  piunp  water  out  of 
both  tanks  and  turn  both  pumps  off.  The  fuzzy  input  state  space  is  composed  of  depth  error  and 
depth  rate,  and  each  is  divided  into  partitions.  The  fuzzy  controller  interpolates  between  the 
partitions  allowing  the  control  to  vary  smoothly  as  the  states  move  fi’om  one  partition  to  another. 
These  movements  of  states  were  provided  by  on  and  off  of  ballast  pumps  [Ref  3]. 

C.  THESIS  MOTIVATION  AND  GOALS 

The  thesis  will  outline  the  development  of  a  depth  controller  using  sliding  mode  control 
techniques  for  a  neutrally  buoyant  vehicle.  The  sliding  mode  controller  is  designed  on  the  basis  of 
the  simplified  four  degrees  of  fi'eedom  vertical  plane  equations  of  motion. 

A  linear  quadratic  regulator  (LQR)  proportional  approach  is  then  utilized  for  the  design  of 
the  ballast  controller,  which  produces  flow  rate  commands,  allowing  the  vehicle  to  have  a  soft 
grounding  behavior. 

These  two  controllers  used  a  logic  based  depth  regulator  to  provide  realistic  simulation  of 
the  vehicle’s  flight  and  grounding  capabilities  for  a  single  mission. 

D.  THESIS  ORGANIZATION 

In  Chapter  H,  the  equations  of  motion  in  six  degrees  of  firedom  are  reviewed  simplified  for 
vertical  plane  case  [Ref.  4],  which  provides  the  basis  for  the  depth  control  design.  Chapter  III 
discusses  the  design  of  a  flight  controller  with  sliding  mode  techniques,  and  the  development  of  the 
grounding  controller  using  different  methods  of  ballast  control.  Chapter  IV  outlines  the  methods 
and  functions  used  for  the  simulating  the  designed  systems.  Chapter  V  discusses  the  results  of  the 
simulation  studies  which  were  conducted  by  using  parameters  of  NFS  Phoenix  AUV.  The 
hardware  components  like  sensors,  pumps,  and  tanks  needed  for  ballast  control  explained  in 
Chapter  VI.  The  conclusions  and  recommendations  can  be  found  in  Chapter  VII. 
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11.  VEfflCLE  MODELING  AND  EQUATIONS  OF  MOTION 


A.  INTRODUCTION 

For  simulation  of  maneuvering  and  motion  control  of  the  vehicle,  it  is  assumed  that 
[Ref  4] : 

1.  The  vehicle  behaves  as  a  rigid  body 

2.  The  earth’s  rotation  is  neglected  as  far  as  acceleration  components  of  the 
vehicle’s  center  of  mass  is  concerned. 

3.  The  primary  forces  that  act  on  the  vehicle  have  inertial,  gravitational  origins  and 
hydrostatic,  propulsion,  thruster,  and  hydrodynamic  forces  from  lift  and  drag. 

B.  DEVELOPMENT  OF  THE  SIX  DEGREES  OF  FREEDOM  NON-LINEAR 
EQUATIONS  OF  MOTION  FOR  A  MARINE  VEHICLE 

The  following  work  concerned  with  development  of  the  six  degrees  of  freedom  non¬ 
linear  equations  of  motion  for  a  marine  vehicle  includes  the  equations  described  by  Healey  [Ref. 
4].  A  vector  x  of  vehicle  body  fiume  velocities  and  a  vector  z  of  global  positions  was  defined 
as; 


u 

V 


w 

P 

q 


r 


and 


X 

Y 

Z 

4> 

e 


(2.1) 


then  considering  M  as  a  6  x  6  mass  matrix  including  translational  and  rotational  inertial  elements, 
the  equations  of  motion  can  be  written  in  the  following  vector  form, 

MS  +/(x)  +  Fg  (z)  =  Fh  (2.2) 


and. 
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z  +  g(x,z)  =  0 


(2.3) 


With  suitable  knowledge  of  the  excitation  force  and  moment  loads,  a  solution  for  the 
vehicle’s  dynamics  can  be  obtained.  A  more  detailed  insight  into  the  development  of  these 
differential  equations,  in  first  order  form  given  by  the  foregoing  analysis  shows, 

X' 

m{v  +  w  X  Pjj }  +  m(w  x  w  x  +  w  x  v) +/g(z)=  Yj-  ,  (2.4) 

X- 

and, 

Kf  ■ 

I^w  +  m{pG  xv}  +  wx(I^w)  +  m{pG  xwxv}  +  mg(z)=  Mf  (2.5) 

X. 

It  helps  here  to  define  the  cross  product  coefficient  matrix  so  that, 

w  X  Po  =  -Pg  X  w  =  -[cros(pG  )]w  (2.6) 

where, 

0  zg  -yo" 

[cros(pG)]=  -Zq  0  Xq  (2.7) 

.Yg  -Xg  0  . 

The  next  step  is  to  collect  all  inertial  terms  into  a  6  x  6  mass  matrix  including  the  inertia 
cross  coupling  effects, 

m  0  0 
0  m  0 
0  0m 

0  -Zg  Yg 
Zg  0  -Xg 

-Yg  Xg  0 

The  remaining  terms  on  the  left  hand  side  arising  from  centripetal  and  coriolis  accelerations 


become. 
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(2.9) 


/(x)= 


m(w.xwx.pQ  +wx.v) 
w  X  .(loW)  +  m{pg  X  w  X  .v} 


The  double  vector  cross  products  are  nonlinear  in  the  primary  velocity  variables  and 
hence  the  need  for  the  nonlinear  functional,  £(•).  The  reader  can  perform  the  indicated 
manipulations  to  express  individual  equations  within  the  set.It  may  also  help  if  the  screw  symetric 
matrix  S(»)  is  used  in  place  of  the  vector  cross  product  [Ref.  5]  .This  yields, 

.  r  m(S(w).S(w).Po+S(w).v  1 
^  [s(w).(I,w)  +  m{S(pG)S(w).v}J 


The  screw  symetric  matrix  involving  the  vector  cross  product  of  ©  with  other  vector  is 


S(«))  = 


0 

r 


-r  +q 

0  -p 

P  0 


(2.11) 


The  components  of  the  sum  of  all  external  forces  and  moments  acting  on  the  vehicle 
body  are  separated  in  the  above  analysis  into  six  components  each  acting  along  the  vehicle  body 
fixed  coordinate  axes  and  form  the  total  vector  of  forces  and  moments  as 

F(t)=[XXt),YXt),Z/(t),KXt),MXt),NXt)]’  .  (2.12) 

where  the  vector  components  in  order  refer  to  the  surge,  sway,  heave  forces,  and  the  roll,  pitch, 
and  yaw  moments  respectively  .The  expressions  for  X/  etc.  are  found  as  the  sum  of  all  external 
forces  acting  on  the  vehicle. 

In  the  component  form,  these  external  forces  can  be  defined  as  follows, 

m[u  +  qw-ru-Xo(q^+r^)  +  yo(pq-f)  +  2G(pr  +  q)]  =  X 

m[u  +  ru-pw-yG(r^  +P^)  +  ZG(qJ'-p)+XG(qp  +  f)]  =  Y  (2.13) 

m[w  +  pu-qu-Zo(p^+q^)  +  XG(rp-q)  +  yG(rq  +  p)]=Z 

where 

F  =  Xi  +  '^  +  Zk  (2.14) 


4 
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(2.15) 


If  I  is  the  mass  matrix  of  inertia  of  the  body,  in  component  form , 

H  =  Hj  +  Hyj  +  H3k 

and, 

Hx  =IxP-Ixyq-Ixzr 
Hy=-I^P  +  I,q-V  (2.16) 

H,  =-I^p-I^q  +  Izr 

If  the  external  moment,  M ,  is  denoted  as, 

M  =  Ki+Mj  +  Nk  (2.17) 

and  in  component  form, 

I,p  +  (I,  -  ly  )qr  +  4  (pr  -  q)  -  Iyz(q'  -  r")  -  I^z  (pq  +  0 

+  m[yG(w-uq  +  up)-Zc(0  +  ur-wp)]  =  K  ,  (2.18) 

lyq  +  (Ix  - Iz)pr  +  K (q’’  +  P)  +  ^yz(pq  "  (P"  -  r' ) 

-m[xG(w-uq  +  up)-Zc(u-ur  +  wq)]  =  M  ,  (2.19) 

I,f  +  ay  -Ix)pq-Ixy(p'  -q')-Iyz(pr  +  q)  +  Ixz(qr-P) 

+  m[xQ  (u  +  ur  -  wp)  -  y,,  (u  -  ur  +  wq)]  =  N  (2.20) 

where, 

Ix  :  vehicle  mass  moment  of  inertia  around  the  x-axis 

(K,M,N)  :  roll,  pitch,  and  yaw  moments,  respectively 
(p,q,r)  :  roll,  pitch,  and  yaw  rates,  respectively 

(  u,  u,  w)  :  surge,  sway,  and  heave  velocities,  respectively 
X  :  body  fixed  axis,  positive  forward 

xg  :  longitutional  position  of  center  of  gravity 

(X,Y,Z)  ;  surge,  sway,  and  heave  forces,  respectively 
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y  :  body  fixed  axis,  positive  starboard 

yo  :  athwartship  location  of  center  of  gravity 

z  :  body  fixed  axis,  positive  down 

zg  :  vertical  position  of  center  of  gravity 

(<j),  0,  v|/)  ;  roll,  pitch,  and  yaw  Euler  angles,  respectively 
These  equations  are  the  three  rotational  equations  of  motion,  and  together  with  the  *  | 

translational  equations,  are  the  six  degrees  of  fi'eedom  equations  of  motion  of  a  rigid  body 
expressed  in  a  coordinate  system  moving  with  the  body,  fixed  at  the  body’s  geometric  center 
[Ref  4], 

C.  EXTERNAL  FORCES  AND  MOMENTS 

The  right-hand  sides  of  Equations  2-13  and  2-20  may  be  expanded  to  include  the  sum  of 
individual  component  forces  and  moments  arising  from  hydrostatic  and  hydrodynamic  sources 
and  external  forces  arising  fi'om  control  surface  deflections  and  propeller  thrust  [Ref  6].  These 
equations  are  expressed  as  follows: 

m[u-or  +  wq-XG(q^  +r^)  +  yG(pq-0  +  ZG(pr  +  q)]  =  XH  +Xw  +Xc 

m[u  +  ur-wq  +  XG(pq  +  f)-yG(p^  +i‘^)  +  ZG(qr-p)]  =  Yh  +Y^  +Yc  (2.21) 

m[w  -  uq  +  up  +  X(,  (pr  -  q)  +  yo  (qr  +  p)  -  (p^  +  q^)]  =  Zy  +  +  Zc 

IxP + (Iz  -  ly  )qr  +  Ixy  (pr  -  q)  -  lyz (q"  -  r' )  -  Ixz  (pq  +  0  (2-22) 

-i-m[yG(w-uq  +  up)-Z(j(u-i-ur-wp)]  =  KH  +Kw  +Kc 

Iyq  +  (I^  -Iz)pr-I^(qr  +  p)-Iy,(pq-r)-I:^(p'  -r^)  (2  23) 

+  m[X(j(w-uq  +  up)-ZQ(u-ur  +  wq)]  =  Mjj  +Mw  +Mc 

Ij  +  (Iy  -IJpq-I^(p'  -q')-Iy,(pr  +  q)-f-I,,(qr-p)  (2.24) 

-i-m[xG(u  +  ur-wp)-yG(u-ur-wq)]  =  Nij  +Nw  +Nc 
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Hydrostatic  restoring  forces  and  moments  are  due  to  the  vehicle  weight  W  and  buoyancy 
B.  The  net  buoyancy  force  in  the  inertia  system  is  (W-B)  in  the  positive  Z-direction  (downwards). 
Therefore,  in  the  ship  fixed  system, 

Xw  =-(W-B)sin9 

Y„  =  (W  -  B)  cos  0  siiK})  (2.25) 

Zw  =  (W-B)  cos  0  cos  (j) 

The  moments  due  to  W  and  B  in  the  component  form, 

=  (y(jW-yBB)cos0cos(|)-(zQW-ZBB)cos0sin<t) 

Mvv  =-(xGW-XBB)cos0cos(|)-(zQW-ZBB)sin0  (2.26) 

Nw  =  (Xg  W  -  XbB)  cos0sin  (j)  +  (y^  W  -  yBB)sin  0 


Forces  and  moments  due  to  control  surfece  deflections  are  reflected  as  added  drag  in 
surge,  while  in  sway,  heave,  pitch  and  yaw  they  are  directly  proportional  to  control  surface 


deflection  [Ref.  6], 

Xc  =  (X,5.5,  +X,3^5Jq  +  X,,^r5,  +X„5^u5,  +(X„s,6,  (2.27) 

+  X5,,,5,^  +X5,5^5,^  +X3^3^5,^  +Xp(n) 


Yc=Y,S,+Y^(n)  (2.28) 

(2.29) 

Kc=Kp(n)  (2.30) 

Mc=M,S,+MsS,S  (2.31) 

Nc=NgS^+Mp(n)  (2.32) 


where, 

=  rudder  deflection 
=  stem  plane  deflection 
=  bow  plane  deflection 

10 


The  terms  with  subscript  P  represents  the  forces  and  moments  generated  by  the  propeller 
revolutions,  n.  The  most  significant  of  these  forces  is  the  propeller  thrust,  Xp{n),  while  Yp,Kp, 
and  Np  represent  small  asymmetry  effects  generated  by  the  propeller.These  three  terms  would 
be  zero  for  the  vehicle  equipped  with  two  symmetrically  loaded  counter-rotating  propellers. 

The  hydrodynamic  forces  and  moments  are  expressed  as  polynomial  functions  of  the 
translational  and  rotational  velocities  of  the  vehicle  with  respect  to  the  water  by  using  a  constant 
coefficient  model, 

Xh  =XppP^  +Xq,q^  +X„r^  +Xp,pr  +  XiU  +  X„qWq  +  X^up  +  X„,ur  (2.33) 

-i-X„„u^ +X„„w^ +R(u) 

Yh  =  Y^p  +  Y,f  +  Y„pq  +  Y^qr  +  Y^u  +  YpP  +  Y,r  +  Y„,uq  +  Y<„p  wp  +  Y„  wr  (2.34) 

+  Y„o  +  Y„„uw 

“^pjbD^h(x)(u  +  xr)"  +  CD^b(x)(w  -  xq)" 

Zh  =Z^q  +  Zppp"+Zp,pr  +  Z„r'+Z*w  +  Zqq  +  Z„pUp  +  Z„,ur  +  Z„w  +  Z„„u"  (2.35) 

bov h(x)(u  +  xr)'  +  CD^b(x)(w  - xq)" dx 

Kh  =K^p+Kff  +  KpqPq  +  Kq,qr  +  K^o  +  KpP  +  K,r  +  K„,uq  +  K^wp  +  K^wr  (2.36) 
-t-K„u  +  K„^uw 

Mh  =  M^q  +  MppP^  +  Mp^pr  +  M„r^  +  M*  w  +  M^q  +  M^up  +  M„,ur  +  w  + 

+  ^pf[CD^h(x)(u  +  xr)"  +CD^b(x)(w-xq)^]^^-^xdx  (2.37) 

Nh  =N^P  +  Nff  +  Np,pq  +  Nq,qr  +  N^,i)  +  NpP  +  N,r  +  N„,uq  +  N^wp  +  N^wr  (2.38) 


+  N„u  +  N„„uw 


-^pj[CD^h(x)(u  +  xr)'  +CD^b(x)(w  -xq)']^j^xdx 

where  R(u)  represents  the  vehicle’s  resistance  curve,  which  is  negative  since  positive  direction  is 
ahead.The  cross  flow  integral  terms  are  integrated  over  the  length  of  the  body  and  they  model 
quadratic  drag  forces.The  cross  flow  velocity  U ^  is, 

Urf  =  [(u  +  xr)^  +  (w  -  xq)^f  (2.39) 

D.  EQUATIONS  OF  MOTION  IN  VERTICAL  PLANE 

We  will  deal  with  only  vertical  plane  variables;  i.e.,  heave,  pitch,  and  surge.The  vertical 
plane  stability  analysis  involves  heave  and  pitch  motions  .However,  the  surge  equation  couples 
into  pitch  and  heave  through  the  metacentric  height  ZG.This  is  a  dynamic  coupling,  and  could  be 
eliminated  by  redefining  hydrod)maniic  coefficients  with  respect  to  the  ship’s  center  of  gravity 
instead  of  its  geometric  center. 

Restricting  the  motions  of  the  vehicle  to  the  vertical  (dive)  plane,  the  only  significant 
motions  that  must  be  incorporated  to  model  the  vehicle  in  the  dive  plane  are,  the  surge  velocity 
(u),  the  heave  velocity  (w),  the  pitch  velocity  (q),  the  pitch  angle  (0)  and  the  global  depth 
position  (z).This  restriction  simplifies  the  previously  developed  equations  to  a  system  of  four 
non-hnear  equations  of  motion,  which  are, 

0  =  q  (2.40) 

(m  -  Z*  )w  +  (-mx 0  -  )q  =  (m  +  Z,  )Uq  +  mz^q^  +  Z„Uw  +  (W  -  B)cos0 

-ipj  C^b(x)^”~--^.fdx  +  U’(Z.^  +<xZ..)5.  (2.41) 

2  i,  |w-xq| 
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(-mxG  -  )w  +  (ly  -  )q  =  (Mq  -  mx  o  )Uq  -  niZo  wq  +  M^Uw  +  (Mg^  +  aMg^  )5 

_lpT r  b(x)^^^^^^xdx-(X(,W-XBB)cose-(zGW-ZBB)sine  (2.42) 
2  i  |w-xq| 

z  =  -Usin0  +  wcos0  (2-43) 


These  equations  can  be  linearized  for  a  level  flight  path  when  the  dive  plane  angle  is 
zero,  5o=0.  By  setting  all  the  time  derivatives  to  zero  and  neglecting  for  the  moment  the 
hydrodjTiamic  drag  terms,  the  following  are  obtained  : 


Z„Uw  +  (W-B)cos0  =  O  (2.44) 

M„Uw-(xQW-XBB)cos0-(zGW-ZBB)sin0  =  0  (2.45) 

q  =  0  (2.46) 

-  U  sin  0  +  w  COS0  =  0  (2-47) 

If  the  vehicle  is  neutrally  buoyant : 

Xg=Xb  (2.48) 

W  =  B 


Then  the  Equations  2.40-2.43  are  linearized  as  : 

0  =  q  (2.49) 

(m-Z,,)w  +  (-mxG  -Z^)q  =  (m+Zq)Uq-HZ,,Uw  +  U%5,  (2.50) 

(-mxo  -M*)w  +  (Iy  -M^)q  =  (Mq  -mxG)Uq  +  M„Uw-(zG  -ZB)W0  +  U^Mg5, 

(2.51) 

z  =  -U0  +  w  (2.52) 

Both  Zs  and  M5  are  a  linear  combination  of  the  respective  stem  and  bow  hydrodynamic 
control  sur&ce  coefficients  and  the  respective  input  value  of  S.This  makes  the  system  of 
equations  as  a  multiple  input  system.To  reduce  this  system  into  a  single  input  system,  the  linear 
combination  of  control  inputs  will  be  modified  into  the  following  form. 
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Z5=Z5^+aZ5^  and  Mj  =M5^ +aMs^ 


(2.53) 


This  will  allow  a  single  input  5  to  control  both  stem  planes  and  bow  planes,  and  will 
cause  the  bow  planes  to  be  slaved  to  the  stem  planes.This  technique  is  known  as  dual  control.The 
value  of  a  will  range  from  -1  to  l.The  selection  of  the  value  of  a  will  allow  the  planes  to  operate 
as  desired  for  the  particular  maneuvering  condition,  i.e.,  a  =  0  for  no  bow  plane  control,  a  =  -1 
for  bow  plane  and  stem  plane  control  opposed  to  each  other,  resulting  the  maximum  pitch 
moment,  and  a  =  1  for  bow  and  stem  plane  control  in  the  same  direction,  resulting  the  maximum 
heave  force. 

These  equations  can  be  shown  in  matrix  format  as  follows. 


"l 

0 

0 

o' 

'e' 

0 

(m-Z^) 

-(mXo+Z.) 

0 

w 

0 

-(mxo  +M*) 

(ly-M^) 

0 

q 

0 

0 

0 

1_ 

z 

o 

o 

■0' 

'o 

0  Z„U  (Z,+m)U  0 

w 

u% 

-(z<3-Z3)W  M„U  (M,-mxJU  0 

q 

-U  1  0  0_ 

z 

0 

‘e’ 

0 

0 

1 

O' 

■0' 

'o 

w 

^21  ^GB 

a^^U 

a23U 

0 

w 

b,U' 

q 

^32  U 

assU 

0 

q 

bjU' 

z 

.  -U 

1 

0 

0_ 

z 

0 

(2.54) 


(2.55) 


where : 

Dv  =  (m-Z^)(Iy  -M^)-(mxG  +Z.)(niXQ  +M^) 


^GB  “ 


^21  “ 


(inx3+Z,)W 
Dv 


(2.56) 

(2.57) 

(2.58) 
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(ly  -  M.)Z^  +(mxg  +Z^)M„ 


(2.59) 


^22  ~  ■ 


Dv 


3-23  —  ' 


(T  -M,)(m  +  Z  )  +  (mX(,  +Z.)(M  -mx^) 


Dv 


(2.60) 


aji  = 


-(m-ZJW 
Dv 


(2.61) 


^32  “ 


(mxe  +M^)Z„  +(m-Z^)M, 
Dv 


(2.62) 


For  the  case  considered  during  this  work,  the  vehicle  has  also  two  ballast  tanks  which 
were  designed  to  be  used  during  the  grounding.  These  ballast  tanks  can  be  seen  in  Figure  2-1. 


The  new  forces  are  6wi  and  6w2  and  since  the  ballast  tanks  are  not  located  in  the  same  distance 
from  the  center  of  gravity  of  the  vehicle  there  will  be  also  two  moments,  L]8wi  and  L28W2.  There 
will  be  also  small  change  in  moment  of  inertia.  So  all  these  changes  can  be  listed  as; 

W  =  Wg  +5w,  H-Swj 

^  _  (W,+5w,+5wJ 
g 

ly  =Iy,  +LA6w,/g)  +  L2^(5w2/g) 

And  the  new  equations  of  motion  become, 

0  =  q 

15 


(2.63) 

(2.64) 

(2.65) 

(2.66) 


(m-Z*)w-(iiiXQ  +Zjq  =  (m  +  Zq)uq  +  mZoq^  +Z„uw  +  (W-B)cos0 


(2.67) 


-ipJC„b(x)(p4  +  u=Z,5. 

2  ■’  [w  -  xq| 

(-mxo  - M* )w  +  (ly  - )q  =  (M,  -  inxo )uq  - mz^wq  +  M„uw  -  (x^W  -  XBB)cose 

- — p  f Cnb(x)-^^^ — ^^^xdx  -  (ZqW  - ZBB)sin  0  +  +  (Ljbwj  -L,5w, )cos0 

2  •’  |w-xq| 

(2.68) 

z  =  -u  sin  0  +  w  COS0  (2.69) 

After  linearization, 

0=:q  (2.70) 

(m-Z^)w-(niXo+Z^)q  =  (m  +  Z,)uq  +  Z,uw  +  (W-B)-|pJCj,b(x)fc^  +  u%63 

|A(V 

(2.71) 

(-mxg  -M^)w  +  (Iy  -M^)q  =  (Mq  -mxo)uq  +  M„uw-(xGW-XBB)-(zGW-ZBB)0 


-^pfCpb(x)^^  xdx  +  u^Mj5^  -L,5w,) 

2  •’  |w  -  xq| 


(2.72) 


Z  =  -U0  +  w 
and  in  matrix  form, 


1  0 

0  (m-Z^) 

0  (-mXg-M^) 

0  0 


0  0 

-(mxo+Z^)  0 

(ly-M,)  0 

0  1 


(2.73) 


(2.74) 


A 


o 


■q  0  1 

O' 

'0 

0 

0 

0  Z„u  (m  +  Zq)Li 

0 

Bo  = 

1 

1 

Z,u^ 

-(zqW-ZbB)  M„u  (Mq-mxoV 

0 

-L,  L, 

My 

-u  1  0 

0_ 

0 

0 

0 

(2.75) 
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U=  5w, 


state  variables  = 


(2.76) 


where ; 


X  =M'AoX  +  M-'BoU 


(2.77) 
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III.  CONTROL  SYSTEM  DESIGN 


Flight  control  and  weight  control  compose  two  main  subsystem  of  a  soft  grounding 
system.  In  flight  control  the  vehicle  is  kept  neutrally  buoyant  and  the  plane  angles  are  the  control 
inputs.  However  in  weight  control,  the  flow  rates  for  both  balast  tanks  are  controlled  with  zero 
forward  velocity  and  plane  angle.  These  two  components  of  the  designed  control  system  were 
explained  in  following  sections. 

A.  FLIGHT  CONTROL 

The  dynamics  of  underwater  vehicles  are  described  by  highly  nonlinear  systems  of 
equations  with  uncertain  coefficients  and  disturbances  that  are  difficult  to  measure.  An  automatic 
controller  for  this  kind  of  vehicle  must  satisty  two  conflicting  requirements:  First,  it  must  be 
sophisticated  enough  to  perform  its  mission  in  an  open  ocean  enviroment  with  ever-changing 
vehicle/environment  interactions.  Second,  it  must  be  simple  enough  to  achieve  real-time  control 
without  nonessential  computational  delays.  Sliding  mode  control  theory  yields  a  design  that 
fulfills  the  above  requirements.  It  provides  accurate  control  of  nonlinear  systems  despite 
uiunodeled  system  dynamics  and  disturbances.  Furthermore,  a  sliding  mode  controller  is  easy  to 
design  and  implement.  A  very  effective  sliding  mode  controller  can  be  developed  from  the 
linearized  equations  of  motion  for  an  imderwater  vehicle  [Ref  7]. 

The  sliding  mode  control  design  problem  can  be  stated  as  follows: 

Given  the  system; 

x  =  f(x)  +  g(x)u  (3.1) 

where  the  state  vector  equation  is, 

■0' 

w 

X  =  (3.2) 

q 

z 
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Choose  the  Lypunov  functions  : 


V(x)=i[cT(x)P  , 


(3.3) 


where 


a(x)  =  s’^x  .  (3.4) 

The  scalar  function  a(x)  can  be  viewed  as  a  weighted  sum  of  the  errors  in  the  states  x. 
For  stability,  it  is  desired  the  time  derivative  of  V(x)  to  be  negative, 

V(x)  =  ad<0,  (3.5) 


This  can  be  achieved  if 


which  means  that 


ad  =  -T|^  a 


a  =  -r|^sign(a) 


(3.6) 


(3.7) 


Using  a(x)  =  s’^x  ,  we  get 


a  =  s^x  =  s^f(x)  +  s^g(x)u  =  -Ti^sign(a)  ,  (3.8) 


and  solving  for  u,  the  control  law  is  obtained, 

u  =  -[s'^g(x)]"' s^f (x)  -  [s^g(x)]'' ii^sign(a)  ,  (3.9) 

In  this  control  law,  the  first  term  is  nonlinear  state  feedback,  and  the  second  term  is  a  switching 
control  law.  The  term  r\^  is  an  arbitrary  positive  quantity,  we  usually  select  it  such  that  V  is 
negative  even  in  the  presence  of  modeling  errors  and  disturbances.  The  above  control  law 
guarantees  stability  of  a(x)  =  0,  or  s^x  =  0.  It  is  necessary  to  find  s.  If  o(x)  =  0  ,  the  system 


becomes 

u  =  -[sVx)rs^f(x)  ,  (3.10) 


and 
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X  =  f  (x)  -  g(x)[s^g(x)]'’  s^f  (x)  . 


when  the  system  is  Unearized, 


X  =  Ax  +  Bu 


(3.11) 


(3.12) 


The  previously  developed  linear  state  matrix  equation  is. 


ri  0 

0 

o' 

'e' 

N 

1 

o 

-(mx^+Z^) 

0 

w 

0  (-niX(--M*) 

(ly-M,) 

0 

q 

0  0 

0 

1 

z 

■  0 
ZdU^ 
MdU 
0 


'0 

0 

1 

o' 

'0' 

0 

ZwU 

(m  +  Zq)i 

0 

w 

-(z^W-z,B) 

M„u 

(m,  -mxo^ 

i  0 

q 

-u 

1 

0 

0 

z 

(3.13) 


1 

0 

0 

0 

M  = 

0  (m 

0  (-mx 

-Zj  - 
g-mJ 

(mxo  +Z,) 

0 

0 

0 

0 

0 

1_ 

fo 

0 

1 

01 

'  0 

0  Z^u 

-(zgW-ZbB)  M^u 

(m  +  Zq^ 

(m,  -mxo 

O  O 

> 

ZdU^ 

MdU" 

L-u 

1 

0 

0 

.  0 

(3.14) 


(3.15) 


where  W  =  since  the  vehicle  is  neutrally  buoyant  during  the  flight.The  above  can  be  written 
as, 

x  =  Ax+Bu  (3.16) 

but  the  dynamics  and  input  matrices  should  be  replaced  by 

A  =  M-’Ao ,  and  B  =  M'’Bo  (3.17) 

and  the  system  is  controlled  with  sliding  mode  controller  where 

u  =  -(s^B)“' s’^  Ax  -  (s^B)”'  Tjsat  sgn(s^x  /  (j))  (3.18) 
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The  closed  loop  dynamics  matrix  is 

Ac=A-B(s^B)-'s^A  =  A-Bk  (3.19) 

then 

k  =  (s^B)-'s^A=>s^Bk  =  s‘^A=>s^A-s^Bk  =  0  ,  (3.20) 

so  the  control  law  becomes  , 

u  =  -kx  -  (s’^B)"'  r|sat  sgn(s’^x  /(|))  (3.21) 

The  gain  vector  k  can  be  found  easily  by  using  Matlab.  The  Matlab  command  place  axxepts  as 
inputs  the  A  and  B  matrices  along  with  a  vector  of  the  desired  closed  loop  poles  and  returns  the 
vector  k. 

B.  WEIGHT  CONTROL 

In  the  second  part  of  the  system,  the  balast  tanks  were  used  to  ground  the  vehicle  on  the 
ocean  floor.  The  vehicle’s  grounding  can  be  simulated  by  adding  weight  proportionally  to  both 
tanks  at  constant  flow  rate  and  by  using  zero  plane  angle  ( <5^  =  0  )  with  those  four  state  variables 

([6,w,q,  z]')  defined  previously.  It  is  needed  to  add  weight  proportionally  to  eliminate  the 
moment  effect  since  these  balast  tanks  are  not  located  in  the  same  distance  from  the  center  of 
gravity.  As  it  can  be  seen  from  the  Figure  3-1,  Li  >  L2. 


Figure  3-1.  The  Location  of  Ballast  Tanks 

To  get  zero  moment, 
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(3.22) 


5w,  =  — *-5w 
L2 

If  the  ballast  pumps  only  pump  water  into  the  tanks  at  fixed  rate,  there  is  no  control  on 
pitch  or  depth  rate  and  pitch  angles  can  develop  large  values.  It  is  proposed  therefore,  that  in 
order  to  keep  the  pitch  angle  within  limits,  flow  rate  of  each  tank  should  be  controlled  separately. 
This  can  be  achieved  by  defining  two  more  states  to  be  added  to  those  existing  four  states.  And 
further  these  two  states  are. 


5w,  =f, 

5W2  = 


(3.23) 


where  SW;  represents  change  of  weight  in  tank  i  and  f  represents  flow  rate  of  pump  i.  So  the 
nonlinear  equations  of  motion  become  : 

e  =  q  (3.24) 


(m-Z*)w  +  (-mX(j  -Z^)q  =  (m  +  Z,)Uq  +  ni2oq^  +Z„Uw  +  (W-B)cose  +  U^(Z5^  +aZs^)6, 


1  u/  \(w-xq)^j 

-pj  CDb(x)-5^ - ^dx 

2  w-xq 


(3.25) 


(-mxo  -M,^)w  +  (I  -M.)q  =  (M  -mxc 


;)Uq-mZoWq  +  M^Uw-ip  f  Cob(x)-^^^ — ^^xdx 

2  i  |w-xq| 


-(xoW-XBB)cos0-(zQW-ZBB)sin0  +  U^(Mg^  +aMg^)5s  (3.26) 
z  =  -U  sin  0  +  w  COS0  (3.27) 


5w,  =f, 

5w2  =f2 


(3.28) 


During  the  grounding  operations,  plane  angles  (5j,)  will  be  zero.  Removing  related  terms,  the 
new  equations  become. 
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0  =  q 

(m  -  )w  +  (-mX(j  -  )q  =  (m  +  Zq)Uq  +  mz^q^  +  Z„Uw  +  (8wi  +  5w 2)  cos 0 


(3.29) 


2 


nose 

p  J  CobCx) 

tail 


(w-xq)^ 

w-xq 


dx 


(3.30) 


(-mxo  -  )w  +  (I  -  )q  =  (Mq  -  mxc  )Uq  -  mzo 


wq  +  M^Uw-ip  J  CpKx)^^  ^^|-  xdx 

2  ii  w-xq| 


-(xqWj, -XBB)cos0-(ZoW-ZBB)sin0  +  (-L,5w,  +L25w2)cos0 
z  = -Usin  0  +  WCOS0 
5w,  =  f, 

dWj  = 
where 


(3.31) 

(3.32) 

(3.33) 

(3.34) 


W  =  Wq  +  5w,  +  5w2 


(3.35) 


When  these  equations  are  linearized, 
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(3.36) 
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Flow  rates  for  balast  tanks  are  control  inputs.  In  the  next  step,  the  control  system  will  be  designed 
to  keep  the  pitch  angle  and  depth  rate  within  the  limits  during  grounding  of  the  vehicle. 

C.  WEIGHT  CONTROL  WITH  LINEAR  QUADRATIC  REGULATOR 
The  system  was  given  as, 

x  =  Ax+Bu  (3.37) 

where  the  gain  matrix  K  of  the  optimal  control  vector, 

u(t)  =  -Kx(t)  (3.38) 

minimizing  the  performance  index  gives 

J  =  £  (x^Qx  +  u^Ru)dt  (3.39) 

where  Q  is  a  positive-definite  Hermitian  matrix  and  R  is  a  positive-definite  Hermitian  matrix. 
(u^Ru)  term  accounts  for  the  expenditure  of  the  energy  of  the  control  signals.  The  matrices  Q  and 
R  determine  the  relative  importance  of  the  error  and  the  expenditure  of  this  energy.  Q  is  the  state 
weighting  matrix  and  R  penalizes  the  control  effort.  It  can  be  choosen  relatively  small  elements  of 
Q  compared  to  R  for  the  control  law  which  will  tolerate  errors  in  x  with  low  control  effort  u. 
Chosing  larger  elements  of  Q  compared  to  R  will  result  in  tight  control  which  means  small  errors 
will  need  considerably  more  control  effort.  The  advantage  of  using  the  quadratic  optimal  control 
system  is  that  the  system  will  be  stable  as  long  as  it  is  conrollable  [Ref.  8]. 

By  using  the  general  equations  of  optimal  control  to  solve  the  LQR  problem  gives  the 
Hamiltonian 

H(x,p,u)  =  p^(Ax+Bu)-^(x^Qx  +  u’^Ru)  ,  (3.40) 

where  p  is  an  unknown  vector  (co-state  vector).  The  necessary  conditions  for  optimality  are. 


(3.42) 


.  .T 

p  =  -— =>p  =  -A*p  +  Qx 

OK 


m 

5u 


=  0  B^p -Ru  =  0  =>  u  =  R‘‘B^p 


(3.43) 


and  the  boundry  conditions  are, 

[p^(tf)+XfF]5Xf  =0  (3.44) 

where  Xy  is  free,  «Sc^  is  arbitrary  and  F  is  symmetric  and  positive-definite  weighting  matrix.  So, 

the  quantity  inside  the  square  brackets  must  be  equal  to  zero  and  this  produces  a  new  form  of 
boundary  condition, 

P(tf)  =  -Fx(tf)  (3.45) 

Now,  the  equations  that  have  to  be  solved  can  be  listed  as  follows. 


X  =  Ax  +BR  'B’^p, 
p  =  Qx  -  A^p, 
x(to)  =  X„, 
p(tf)  =  Fx(t,). 


(3.46) 


From  above  equations,  p(t)  can  be  calculated  and  this  will  provide  u  as  a  fimction  of  time  from 
u  -  .  By  using  Kalman’s  idea,  p(t)  can  be  defined  as, 

p(t)  =  -S(t)x(t) 

where  S(t)  is  a  symmetric  positive-definite  matrix.  Also, 

p  =  -Sx  -  Sx  =  -Sx  -  S(Ax  -fBR"'B^p) 

and , 

Qx  -  A^p  =  -Sx  -  SAx  -  SBR-'B^p 
-  Sx  =  (A^S  +  SA-  SBR-’B^S  +  Q)x 

and  for  this  to  be  true  for  all  x. 


(3.47) 

(3.48) 

(3.49) 

(3.50) 
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-S  =  A'^S  +  SA-SBR-'B^S  +  Q  (with  S(tj.)  =  F) 


(3.51) 


This  is  called  Riccati  matrix  differential  equation  and  S(t)  can  be  obtained  by  backwards 
integration  of  this  equation.  Since  u  =  R~'B^ p{t)  the  closed  loop  optimal  control  law  can  be 
found  by 

u  =  R-'B^Sx  (3.52) 

where  S  can  be  found  by  solving  the  algebraic  Riccati  equation  for  the  positive-definite  S , 

A^S  +  SA-SBR-‘B''S  +  Q  =  0  (3.53) 

In  Matlab,  the  command 

lqr(AB,Q,R) 

solves  the  continuous-time,  linear,  quadratic  regulator  problem  and  the  associated  Riccati 
equation.  This  command  calculates  the  optimal  feedback  gain  matrix  K  for  control  law  which 
minimizes  the  performance  index. 

D.  GROUNDING  WITH  VERTICAL  THRUSTERS 

The  bladed  thrusters  are  the  essential  elements  of  improved  vehicle  positioning  systems. 
With  automatic  position  control,  the  thrusters  enable  important  scientific  and  industrial  tasks  such 
as  automatic  docking,  station  keeping,  precise  surveying,  inspection,  sample  gathering  and 
manipulation.  Incorporating  precise  models  of  thruster  dynamics  into  the  feedback  control 
systems  of  marine  vehicles  promises  improved  vehicle  positioning  [Ref.  9]. 

Most  small-to-medium  sized  underwater  vehicles  are  powered  by  electric  motors  driving 
propellers  mounted  in  ducts.  The  propeller  is  mounted  in  a  duct  or  shroud  in  order  to  increase  the 
static  and  dynamic  efficiency  of  the  thruster.  Thrusters  are  subject  to  serious  degradation  due  to 
axial  and  cross  flow  effects.  Axial  flow  effects  can  be  reasonably  approximated  by  the  modeling 
of  the  thruster  unit  alone,  the  velocity  of  the  fluid  entering  the  thruster  shroud  effectively  changes 
the  angle  of  attack  of  the  propeller ,  thus  altering  the  force  produced.  Cross  flow  effects  are  much 
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more  difficult  to  model  and  are  highly  dependent  on  the  position  of  the  thruster  on  the  vehicle. 
The  amoimt  of  force  produced  by  the  thruster  will  reduce  the  overall  gain  of  a  control  system 
unless  these  effects  are  specifically  in  the  controller  design[Ref  10]. 

For  this  work,  NFS  Phoenix  vehicle  is  taken  as  an  example.  Figure  3-2  shows  the 
locations  of  vertical  and  horizontal  thrusters  on  the  vehicle.Those  four  tubes  represents  the 
thruster  shrouds.  In  Figure  3-3,  the  vertical  thruster  tubes  can  be  seen  throughout  the  vehicle. 
Thruster  blades  are  located  close  to  the  bottom  of  those  tubes. 

Thruster  moment  and  force  equations  were  developed  by  Louis  L.Whitcomb  and  Dana 
R.Yoerger  [Ref  9].  On  that  paper,  the  control  system  for  these  thrusters  was  also  discussed.  But 
in  this  study,  these  thruster  force  and  moments  were  assumed  as  some  constant  parameters  and 
also  no  control  law  w?as  developed  to  control  them.  Since  the  main  element  for  grounding  is  the 
weight  control,  the  thrusters  were  just  used  as  auxiliary  elements  of  this  procedure  in  order  to 
increase  depth  rate.  By  using  thrusters  in  addition  to  the  weight  control,  following  changes 
should  be  done  to  heave  and  pitch  rate  equations, 

(m  - )w  +  (-mxo  - Z .  )q  =  (m  +  Z^jUq  +  mz^q"  +  Z,,Uw  +  (8w,  +  5w2)cose 

-lp7Cpb(x)<”~’‘^{dx  +  Z^  (3.54) 

(-mxo  -M,^)w  +  (Iy-M^)q  =  (Mq-mxG)Uq-mzGwq  +  M„Uw--p  J  CpbCx)  xdx 

-(XqW^  -XBB)cos0-(zGW-ZBB)sine-i-(-L,5w,  +L26w2)cose-Ma^^„q  (3.55) 
where  Zt^rusterand  Mthrvster  are  thruster  force  and  thruster  moment  respectively. 


28 


Figure  3-2.  Horizontal  and  Vertical  Thrusters  of  NFS  Phoenix  AUV 


Figure  3-3.  Vertical  Thrusters  and  Thruster  Shrouds  of  NFS  Phoenix  AUV 
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IV.  SIMULATION 


For  the  realistic  simulation  of  the  grounding  system,  some  other  control  functions  were 
used  in  addition  to  the  flight  and  weight  controls.  The  system  was  simulated  by  using  Matlab’s 
Simulink  program.  For  the  dynamic  control,  Matlab’s  s-function  was  used.  This  tool  gives  us  the 
opportunity  to  use  the  memory  dynamically.  In  feet,  it  is  nothing  different  than  a  ODE  solver.  In 
that  block,  the  state  equations  were  solved  by  using  Matlab’s  ode45  function  with  variable  step 
size. 

As  depicted  in  Figure  4-1,  two  main  components  of  control  system  are  flight  control  and 
weight  control,  and  they  also  form  two  main  loops  of  simulation. 


t 

O 

I 


Figure  4-1.  Dynamic  Controller  with  Weight  and  Flight  Controllers 
For  the  simulation  of  longitudinal  motion,  in  addition  to  the  states  used  in  the  control 
laws,  X  was  also  included.  But  this  term  was  not  used  in  control  design.  X  was  defined  as, 

X  =  u.cos6  +  wsin0  +  Ucz  (4.1) 

where  u  is  the  forward  speed  and  Uez  is  current. 

The  design  of  weight  and  flight  controls  were  explained  in  previous  chapter.  Zcom  is  the 
commanded  flight  depth.  The  sliding  mode  controller  calculates  the  control  surfaces  from  the 
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error  which  is  the  difference  between  the  commanded  and  actual  depths.  The  switch  from  the 
flight  control  to  the  weight  control  is  decided  by  the  forward  speed  command  produced  by  speed 
control  unit  This  unit  has  two  parts;  Speed  control  and  secondary  control  units. 

The  speed  control  block  seen  in  Figure  4-2  reduces  the  forward  velocity  to  0.1  ft/sec 
when  the  vehicle  reaches  to  the  commanded  flight  depth.  But  this  is  not  a  sudden  decrease  in  the 
speed.  Since  the  longitudinal  position  was  also  commanded  by  Xcom  block,  the  control  unit 
calculates  the  necessary  break  distance  in  order  to  reach  that  location.  The  break  distance  was 
determined  from  a  known  deceleration  in  the  longitudinal  direction  and  the  time  needed  to 
ground  from  the  flight  depth.  As  seen  on  the  Figure  4-2,  the  input  parameters  to  the  speed  control 
block  are  current  and  commanded  depths,  current  and  commanded  longitudinal  locations, 
commanded  forward  speed  and  ground  depth;  the  output  of  the  block  is  the  controlled  forward 
speed. 


Speed  Control 


Figure  4-2,  Speed  Control  Block 

As  mentioned  above,  the  main  control  input  for  the  speed  control  block  is  the  depth. 
When  the  depth  error  (Zcom  -  Zcuit)  becomes  smaller  than  a  certain  number,  the  deceleration 
procedure  starts.  One  problem  with  this  method  is  at  the  beginning  of  the  grounding,  the  depth 
error  starts  to  increase  again  which  means  that  the  speed  control  will  increase  the  forward 
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velocity  until  it  is  equal  to  commanded  speed.  To  prevent  this,  the  secondary  control  imit  seen  in 
Figure  4-3  was  designed: 


Secondary  Control 


Figure  4-3  Secondary  Speed  Control  Block 

Two  of  the  five  inputs  of  this  block  are  the  weights  in  both  ballast  tanks.  If  the  weight  in  one  or 
both  of  those  two  tanks  are  different  than  zero,  then  the  output  of  the  secondary  control  becomes 
0.1  (forward  speed).  Otherwise,  the  block  passes  the  value  coming  from  the  speed  controller 
without  changing. 

The  speed  command  produced  by  the  secondary  controller  provides  the  switch  from 
flight  to  the  grounding  procedures.  Figure  4-4  shows  secondary  control,  plane  angle  controller 
and  weight  controller  blocks. 


Mode  controller 


Figure  4-4.  The  Relationship  of  Secondary  Control  Block  with  Plane  Angle  and 


Weight  Controller  Blocks 
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When  the  forward  velocity  is  reduced  to  0.1  ft/sec,  the  weight  controller  starts  to  produce 
flow  rate  commands  by  using  the  methods  described  in  Chapter  III.  The  plane  angle  control  block 
passes  the  value  coming  from  the  sliding  mode  controller  (flight  control  block)  without  changing. 
But  with  the  reduced  speed,  it  makes  the  plane  angle  zero.  So  this  ends  the  effect  of  flight  control 
on  the  vehicle,  in  other  words,  the  weight  controller  gets  the  control  on  the  vehicle’s  motion. 

Figure  4-5  shows  all  functions  mentioned  above  with  flight  and  weight  controllers  as  a 
complete  Simulink  diagram.  The  flow  chart  in  Figure  4-6  shows  the  logic  of  the  procedure.  The 
Matlab  programs  used  in  the  simulation  are  presented  in  Appendix  A. 


Figure  4-5.  Simulink  Block  Diagram  Used  for  Simulation 


34 


36 


V.  RESULTS  AND  DISCUSSION 


A.  INTRODUCTION 

In  previous  chapters,  the  mathematical  models  of  the  control  system  were  developed.  To 
prove  the  validity  of  the  flight  and  weight  controllers,  the  system  was  simulated  by  using  the 
parameters  of  NFS  Phoenix  AUV.  These  parameters  are  presented  in  Appendix  B.  First  the  AUV 
was  controlled  by  flight  controller  (sliding  mode  control)  during  its  diving  from  surface  to  a 
commanded  flight  depth.  Second,  the  flight  controller  and  the  different  cases  of  weight  controller 
were  simulated  on  the  vehicle. 

B.  FLIGHT  CONTROL 

In  the  NFS  Phoenix  vehicle  ,  there  are  four  vertical  control  planes  powered  by  servo 
motors.  Using  the  definitions  in  Chapter  II  and  Chapter  HI  with  the  parameters  of  NFS  Phoenix 
AUV  and  a  nominal  speed  of  4  ft/sec,  A  and  B  matrices  becomes 

0  0  1  o]  To  ■ 

0.0155  -3.4119  -0.9247  0  -0.7139 

-0.1086  0.6027  -0.9667  0  ~  -0.0123 

-4  1  0  oj  . 


By  choosing  the  poles  as  p  =  [-2,  -2.1,  -2.2,  0],  the  vector  k  was  calculated, 

k  =  place(A,B,  [-2,  -2.1,  -2.2,  0]) 
k  =  [-18.3998  ,  -2.4217 ,  -15.6369 ,  0] 

Ac  is  calculated  from 


Ac  =  A-Bk 


0  0  10 

-13.1205  -5.1408  -12.0882  0 

-0.3352  0.5729  -1.1592  0 

-4.0  1  0  0 


(5.2) 

(5.3) 

(5.4) 

(5.5) 
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The  eigenvector  of  A^c  for  the  pole  at  the  origin  is  the  sliding  surfece 

s  =  [-0.8733  ;  -0.0272  ;  -0.4692 ;  0.1287] 


(5.6) 


as  a  result  with  (j>  =  0.1  ,  the  control  law  becomes 
6  =  -18.39980  -  2.4217©  - 15.63 19q 

-  (0.4)satsgnJ-  0.87336  -  0.0272®  -  0.4692q  +  0. 1287(z  -  ]/0.  l}  (5.7) 

The  motion  of  the  vehicle  is  restricted  to  the  vertical  plane.  The  motion  profiles  for  depth 
and  pitch  have  been  specified  using  sliding  mode  control.  For  the  maneuver,  the  commanded 
depth  was  4  ft  and  the  vehicle  was  originally  at  the  surfece.  As  can  be  seen  fi'om  the  Figure  5-1, 
during  the  flight,  maximum  pitch  angle  becomes  0.042  rad  (~  2.5  degrees).  When  the  vehicle 
reaches  to  the  commanded  depth  as  seen  in  Figure  5-2,  the  pitch  angle  becomes  zero  as  expected  . 
The  controller  produces  the  dive  plane  angle  command  according  to  the  depth  error.  In  the 
beginning  the  depth  error  is  large,  so  the  system  produces  higher  values  of  plane  angle  command 
in  order  to  eliminate  this  error.  With  the  full  state  feedback,  the  vehicle  responded  very  well  to 
these  commands. 

C.  ADDING  WEIGHT  TO  BOTH  TANKS  WITHOUT  CONTROL 

After  completion  of  the  flight  to  the  commanded  depth,  the  vehicle  gets  water  to  both 
tanks  in  order  to  become  heavy.  Figure  5-3  shows  the  weight  increase  in  the  tanks.  During 
grounding,  the  planes  kept  at  zero  degrees  as  depicted  in  Figure  5-4.  To  keep  depth  rate  within 
limits,  the  maximum  weight  pumped  in  was  limited  at  5  lb  for  each  tank.  With  this  additional 
weight,  the  vehicle  sat  on  the  ground  (10  ft.)  with  0.6  ft/sec  depth  rate.  Eventhough  the  weight 
was  added  proportionally  (Swj  =  (L,  /Lj  )5w, )  to  get  zero  momentum  effect,  there  is  still  some 

momentum  because  of  the  vehicle  itself  This  momentum  was  created  by  the  pitch  and  heave 
motion  of  the  vehicle.  As  a  result,  the  pitch  angle  increases  since  there  is  no  control  on  either 
depth  rate  or  pitch  angle.  As  seen  on  Figure  5-5  and  Figure  5-6,  at  the  end  of  a  6  ft.  drop,  the  pitch 
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angle  becomes  0.6  rad.  (35  degrees).  This  method  can  be  used  for  very  short  grounding  depths  (2- 
3  ft),  but  for  other  cases,  it  is  not  recommended  since  the  system  is  completely  unstable. 


Pitch  angle  vs  time 


Figure  5-1.  Pitch  Angle  Plot  During  The  Flight 


Figure  5-2.  Depth  Change  As  A  Function  Of  Time  During  The  Flight. 
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Weight  in  balast  tank  (lb) 


5 


Weight  in  balast  tank  vs  time 


Figure  5-3.  Weight  Addition  During  Grounding  With  No  Control  On  Flow  Rates 
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Figure  5-4.  Plane  Angle  During  Flight  And  Grounding 
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Figure  5-5.  Depth  Change  During  The  Flight  And  Grounding 
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Figure  5-6.  Pitch  Angle  Change  For  Flight  And  Grounding 
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D.  WEIGHT  CONTROL  WITH  LINEAR  QUADRATIC  REGULATOR 


In  order  to  keep  the  depth  rate  and  pitch  angle  within  limits,  the  linear  quadratic  regulator 
technique  was  used  in  designing  a  weight  control.  The  mathematical  model  for  this  control 
system  was  previously  explained  in  Chapter  HI.  The  parameters  of  NFS  Phoenix  vehicle  was 
used  for  simulation.  With  these  known  parameters  and  0.1  fl/sec  forward  velocity,  A  and  B 
matrices  become. 
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In  practice,  high  values  of  pitch  angle  (>  15  degrees)  are  not  desired  for  a  safe  and  stable 
grounding  of  the  vehicle.  So  the  designed  control  law  should  not  tolerate  too  much  oscillations  of 
pitch  angle.  This  kind  of  control  law  can  be  provided  by  choosing  larger  elements  of  Q  for  pitch 
angle  compared  with  the  others.  So,  Q  and  R  matrices  were  chosen  as  follows; 
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(5.9) 


(5.10) 


By  using  Iqr  command  in  Matlab,  control  gain  matrix  K  can  be  obtained, 

[K,s,m]=lqr(A,B,Q,R) 


(5.11) 


and  with  the  numbers  provided , 
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(5.12) 


0.0608  0.1263  -  0.0526  0.0138  0.0754  0.0365 

-0.0322  0.1294  0.0021  0.0120  0.0365  0.0613 


Since  grounding  to  the  ocean  floor  from  a  certain  depth  desired,  the  command  matrix  should  be 
Xcom  =[000^00]’  where  Zgr  =  (ground  depth)  -  (the  depth  where  grounding  is  started).  So  the 
control  law  becomes , 

U  =  -Kx«„.  (5.13) 


where 

Xetror  ^  ”  ^com  •  14) 

The  simulation  of  the  system  with  this  control  law  can  be  seen  in  Figure  5-7  through 
Figure  5-14. 

Positive  flow  rate  represents  water  inlet  to  the  balast  tanks,  and  negative  flow  represents 
the  opposite.  The  pumps  are  not  allowed  to  pump  out  when  there  is  no  water  in  balast  tanks. 
Figure  5-7  and  Figure  5-8  show  when  the  weight  in  a  balast  tank  and  the  pump  flow  rate  become 
zero.  This  is  provided  by  a  simple  controller  which  compares  the  weight  in  ballast  tank  (5w)  and 
flow  rate  (f).  If  8w  is  equal  to  zero  and  flow  rate  is  a  negative  number,  than  the  control  input  (f) 
of  that  pump  becomes  zero. 

At  the  commanded  depth,  the  speed  control  unit  slows  down  the  vehicle  to  an  almost  zero 
forward  velocity  (u  =  0. 1  ft/sec).  The  speed  control  unit’s  other  duty  is  to  control  the  longitudinal 
position.  During  the  flight,  the  speed  control  unit  compares  the  vehicle’s  location  (x)  with 
commanded  location  (xcom)  which  is  a  longitutional  distance  from  the  original  position.  When  the 
vehicle  is  at  the  commanded  depth  of  flight,  a  deceleration  procedure  starts.  A  simple  algorithm 
was  used  to  calculate  the  minimum  distance  needed  for  deccaleration  to  reach  the  commanded 
location  at  the  end  of  the  grounding.  The  change  of  forward  velocity  due  to  the  depth  change  can 
be  seen  in  the  Figure  5-13. 
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The  depth  rate  as  seen  in  Figure  5-1 1  is  very  low  in  this  method  because  of  the  command 
given  to  the  weight  control.  The  weight  control  produces  its  control  values  due  to  the  errors 
which  are  the  differences  between  the  commanded  and  the  actual  states.  In  the  above  case,  only 
the  depth  (z)  command  has  a  value,  the  commands  for  other  states  are  zero.  At  the  end  of  the 
simuation,  the  pitch  angle  becomes  almost  zero  as  seen  in  Figure  5-12.  In  the  first  half  of  the 
grounding.  Figure  5-9  and  Figure  5-10  shows  an  increase  in  weight  for  both  tanks,  but  in  the 
second  half  the  system  tries  to  make  the  vehicle  neutrally  buoyant  as  expected. 

E.  LQR  WITH  POSITIVE  WEIGHT  COMMAND 

So  when  the  vehicle  reaches  the  ground,  there  will  be  almost  no  water  in  balast  tanks.  But 
for  the  stability  of  the  grounding,  the  vehicle  should  be  heavier.  For  this  reason,  in  addition  to  the 
depth  command,  the  weight  can  also  be  commanded  to  increase  the  depth  rate  or  the  weight  of  the 
vehicle  at  the  end  of  the  grounding.  So,  the  command  vector,  Xcom  is  changed  to , 

Xcom  ~  [0  >  0 , 0  ,  Zground  5  SWcoml  5  5Wcom2] 

where  5wcom  are  some  positive  numbers  and  represent  the  command  for  additional  weight  and  the 
system  was  simulated  with  these  new  parameters.  Figure  5-15  through  Figure  5-20  show  the  plot 
of  this  simulation.  In  this  case,  pitch  angle  reaches  a  maximum  value  of  0.18  rad  (10.31  degrees) 
which  is  4  times  greater  than  the  previous  simulation.With  increasing  pitch  angle,  there  is  also  an 
increase  in  the  depth  rate.  The  depth  rate  becomes  0.35  fl/sec  which  is  again  almost  4  times 
greater  than  the  previous  simulation.  Since  the  commanded  depth  for  the  control  law  is  the  depth 
of  the  ground,  the  system  tries  to  make  the  vehicle  neutrally  stable  to  keep  the  vehicle  on  that 
depth  by  pumping  water  out  of  balast  tanks.  After  grounding,  pumps  should  pump  water  in  balast 
tanks  until  they  are  full.  Because  this  additional  weight  is  needed  to  keep  the  vehicle  sitting  on  the 
ground  against  the  current. 


Another  method  for  increasing  the  depth  rate  is  to  command  with  a  depth  value  which  is 
greater  than  the  actual  ground  depth.  Because  in  the  beginning  the  error  will  be  higher,  the  weight 
controller  will  produce  higher  values  of  flow  rate. 
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Figure  5-7.  Flow  Rate  As  A  Function  Of  Time  For  Pump-1  With  Depth  Command  Only 
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Figure  5-8.  Flow  Rate  As  A  Function  Of  Time  For  Pump-2  With  Depth  Command  Only 
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Figure  5-9.  Weight  Change  In  Ballast  Tank-1  With  Depth  Command  Only 
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Figure  5-10.  Weight  Change  In  Ballast  Tank-2  With  Depth  Command  Only 
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Figure  5-11.  Depth  Change  During  Flight  And  Groimding 
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Figure  5-12.  Pitch  Angle  Change  With  Depth  Command  Only 
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Change  in  depth  and  forward  velocity 


Figure  5-13.  Comparison  Of  Depth  And  Forward  Velocity  Change 


Distance  from  the  original  position  vs  depth 


Figure  5-14.  Response  Of  The  Vehicle  To  The  Longitudinal  Position  Command 
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Figure  5-15.  Flow  Rate  For  Pump-1  With  Weight  And  Depth  Commands 
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Figure  5-16.  Flow  Rate  For  Pump-2  With  Weight  And  Depth  Commands 
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Weight  in  tank-1  vs  time 


Figure  5-17.  Weight  Change  In  Ballast  Tank-1  With  Depth  And  Weight  Commands 
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Figure  5-18.  Weight  Change  In  Ballast  Tank-2  With  Depth  And  Weight  Commands 
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Depth  vs  time 
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Figure  5-19.  Depth  Change  With  Weight  And  Depth  Commands 


Figure  5-20.  Pitch  Angle  As  A  Function  Of  Time  With  Weight  And  Depth  Commands 


51 


F.  GROUNDING  WITH  THRUSTERS  IN  ADDITION  TO  WEIGHT  CONTROL 

NFS  Phoenix  AUV’s  cross-body  thrusters  consist  of  a  3  in.  ID  aluminum  tube  with  a 

centrally  located  4  blade  brass  propeller.  A  spur  gear  is  mounted  aroimd  a  3  in.  diameter  propeller 
and  driven  by  a  pinion  cormected  to  a  24  Vdc  motor  giving  a  2.5:1  gear  reduction.  The  twist  of 
the  propeller  blade  is  symmetric  enabling  bi-directional  operation  delivering  approximately  1.0 
pound  of  bollard  pull  force  in  either  direction  [Ref  11].  The  system  was  simulated  with  the  new 
state  equations  described  in  Chapter  III  and  the  same  weight  control  conditions  defined  in  section 
C  of  this  chapter.  Figure  5-21  through  Figure  5-26  show  the  result  of  this  simulation.  The  depth 
rate  becomes  0.5  ft/sec  and  the  pitch  angle  reaches  a  higher  value  since  there  is  no  control  on 
thrusters.  Because  the  thrusters  are  dominant  in  this  case,  the  weight  controller  losses  most  of  its 
effect  on  pitch  control. 

G.  BOTTOM  STABILITY 

The  ground  also  affects  the  vehicle  closing  to  the  bottom.  The  theory  explained  by 
Hoemer  and  Borst  [Ref.  12],  predicts  that  roughly  below  Cl  =1.5  (Cl  represents  lift  coefficient), 
hft  will  be  increased  in  proximity  of  the  ground.  Eventhough  no  experimental  data  is  provided  for 
NFS  Phoenix  AUV,  it  can  be  assumed  that  hft  coefficient  will  be  less  than  1.5.  For  study  of  the 
bottom  stabihty,  two  different  cases  of  grounding  were  considered.  Figure  5-27  shows  these  two 
cases. 

In  the  case  that  the  vehicle’s  stem  touched  to  the  bottom  first,  the  lift  will  decrease  the 
weight  and  inrease  the  angle  of  attack.  This  reduces  the  stability  and  makes  grounding  more 
difficult.  But  this  feature  can  be  very  helpful  when  leaving  the  ground.  In  the  other  case,  the  bow 
of  the  vehicle  touches  the  ground  first.  This  time  lift  makes  the  vehicle  heavier  and  decreases  the 
angle  of  attack  providing  more  stable  grounding.  After  the  completion  of  grounding  process,  the 
vehicle  sits  on  the  bottom  with  no  lift  since  the  lift  coefficient.  Cl  is  assumed  zero  because  of  the 
symmetric  shape  of  the  NFS  Phoenix  AUV.  Figure  5-28  shows  the  ocean  current  that  the  vehicle 
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can  stand  with  different  ballast  weights  sitting  on  soil  with  0.7  friction  coefficient.  When  both 
ballast  tanks  are  filled  with  water  completely  (-23.4  lb  water  in  each  tank),  the  vehicle  can  keep 
its  position  against  1.52  m/sec  (-3  knots)  of  current. 
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Figure  5-21.  Flow  Rate  For  Pump-1  With  Thrusters  And  Weight  Control 
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Figure  5-22.  Flow  Rate  For  Pump-2  With  Thrusters  And  Weight  Control 
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Figure  5-23.  Weight  Change  In  Ballast  Tank-1  With  Thrusters  And  Weight  Control 
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Figure  5-24.  Weight  Change  In  Ballast  Tank-2  With  Thrusters  And  Weight  Control 
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Figure  5-27.  Forces  Acting  On  The  Vehicle  In  Two  Cases  Of  Grounding 
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Figure  5-28.  The  Current  That  Vehicle  Can  Keep  Its  Position 

56 


VI.  PROPOSED  HARDWARE  IMPLEMENTATION 


For  NPS  Phoenix  Vehicle,  a  full  description  of  all  sensors  and  other  hardware 
components  which  were  present  on  the  vehicle  were  given  by  Marco  [Ref.  11].  It  covers  the 
gyroscopes,  speed  sensor,  short  baseline  navigation  system  and  GPS  components.  These 
components  are  used  to  sense  vehicle  roll,  pitch  and  heading  angular  positions,  depth,  forward 
speed  and  location  in  global  coordinates.  Two  open  screws  located  at  the  stem,  two  vertical  and 
two  lateral  cross-body  thrusters,  eight  control  surfaces  (four  radders  and  four  vertical  planes) 
control  these  states.  In  the  new  design  with  dynamic  ballast  control,  two  tanks  (bow  and  stem), 
two  water  pumps,  and  two  flow  meters  as  sensors  were  added  to  the  vehicle.  Those  tanks  and 
pumps  were  already  shown  on  Figure  3-1.  More  detailed  design  can  be  seen  on  Figure  6-1. 


Figure  6-1.  Hardware  Components  For  Ballast  Control  System 
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The  pump  provides  one  way  continuous  water  delivery  with  its  positive  displacement,  3 
chamber  diaphragm.  Its  maximum  capacity  is  1.75  gpm  with  12  nominal  dc  voltage.  Since  the 
pump  works  only  one  way,  a  four  connection,  three  position,  spring  centered,  selonoid-control 
valve  is  used  to  change  the  flow  direction.  By  simply  changing  the  position  of  the  valve,  the  same 
pump  can  be  used  to  pump  water  both  in  and  out  of  the  ballast  tank. 

Flow  meters  will  be  used  as  sensors  to  provide  flow  rate  information  to  the  control 
system.  Flow  rate  will  be  calculated  from  the  pressure  difference  between  the  two  ends  of  the 
meter  and  this  analog  data  will  be  converted  to  digital  one  in  A/D  converter  in  order  to  be  used  in 
the  main  computer  of  the  vehicle. 
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VIL  CONCLUSIONS  AND  RECOMMENDATIONS 


A.  CONCLUSIONS 

•  A  method  for  modeling  and  simulation  of  a  soft  grounding  system  for  an  imderwater 
vehicle  has  been  developed  and  presented.  This  study  outlined  three  different  sections  of  that 
system,  flight  control,  weight  control  and  bottom  stability. 

•  The  critical  parameters  are  pitch  angle,  depth  rate  and  pump  flow  rate.  The  limitations 
of  these  parameters  relative  to  the  new  NFS  AUV  have  been  determined  and  a  feasible  design  has 
been  validated  by  simulation. 

•  An  LQR  state  feedback  control  provided  sufficient  weight  keeping  to  maintain  the 
vehicle  pitch  angle  within  the  limits,  but  the  depth  change  rate  is  limited.  For  a  higher  depth  rate, 
the  vertical  thrusters  should  be  used  in  addition  to  the  weight  control.  This  uses  more  energy, 
however,  which  may  be  unacceptable  in  practice. 

•  In  ocean  currents,  the  bottom  stability  depends  on  vehicle’s  direction  and  angle  of 
attack.  For  stable  grounding,  the  vehicle  would  commanded  to  face  the  opposite  direction  of  the 
current.  Bottom  conditions  are  highly  variable,  but  an  estimate  indicates  that  stability  for  currents 
less  than  1  knot  may  be  possible. 

B.  RECOMMENDATIONS 

The  most  important  recommendation  and  the  next  step  should  be  to  perform  experiments 
in  Monterey  Bay  with  added  weight  in  the  vehicle.  With  the  Sontek  ADV  velocimeter  to  measure 
water  velocity,  these  experiments  will  determine  weight  requirements  for  bottom  stability  in 
typical  ocean  conditions. 
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APPENDIX  A 


The  matlab  programs  used  to  simulate  the  dynamic  behavior  of  the  NPS  Phoenix  are 
given  in  this  appendix 
MATLAB  PROGRAMS 

%  -  .  . .  .■■._^==== . .  , 

%  denemel.m  is  the  main  function  of  the  dynamic  system.  It  defines  the  parameters,  variables 
and 

%  state  vectors 

%  -  -  ' . . . " . ,.^3:=====  . . 

function  [sys,xO,str,ts]  =  denemel(t,x,u,flag) 
global  Ao  Zqdot  Zwdot  Mqdot  Mwdot  Wo  g  lyo  L 

%  The  parameters  used  for  Ae  simulation 
%  The  physical  parameters  of  Ae  veAcle 
L  =94.0/12.0; 

LI  =24.25/12.0; 

L2  =  14.75/12.0; 

Wo  =435.0; 
g  =32.174; 

Bu  =Wo; 
lyo  =  42.0; 
rho  =  1.94; 
xG  =0.0; 
xB  =xG; 

ZG  =0.5/12.0; 

ZB  =  0.0; 

ZGB  =  ZG-ZB; 

%  The  hyAodynamic  coefficients 
Zqdot  =  -0.00253*2756.81; 

Zwdot  =  -0.09340*377.67; 

Zq  =-0.07013*377.67; 

Zw  =-0.78440*51.72; 
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Zds  =-0.02110*51.72; 

Zdb  =-0.02110*51.72; 

Mqdot  =  -0.00625*20137.50; 

Mwdot  =  -0.00253*2756.81; 

Mq  =-0.01530*2756.81; 

Mw  =  0.05122*377.67; 

Mds  =-1.7664; 

Mdb  =  1.3260; 

%  The  bow  and  stem  plane  relations 
a=  1; 

ZGB  =  ZG-ZB; 

Zd  =  (Zds  +  a*Zdb); 

Md  =  (Mds  +  a*Mdb); 

%  A  and  B  matrices  were  used  to  pass  values  to  the  other  functions  of  dynamic  system. 

A=[0  01  0  0  0  0;Bu  xG  xB  ZB  0  0  0;1  Zw  Zq  1  0  0  0;  ZG  Mw  Mq  1  0  0  0;1  1  0  0  0  0  0; 

0  0  0  0  0  0  0;0  0  0  0  0  0  0]; 

B=[0  0  0;1  1  Zd;Ll  L2  Md;0  0  0;0  0  0;0  0  0;0  0  0]; 

%  Since  the  system  is  full  state  feedback,  C  is  a  diagonal  matrix 

C=[l  0  0  0  0  0  0;0  1  0  0  0  0  0;0  0  1  0  0  0  0;0  0  0  1  0  0  0;0  0  0  0  1  0  0;0  0  0  0  0  1  0;0  0  0  0  0  0  1]; 
D=zeros(7,5); 

switch  flag, 

%  Dispatch  the  flag.  The  switch  fimction  controls  the  calls  to  S-fimction  routines  at  each 
simulation  stage 

%  Initialization 

%  sys  is  a  generic  return  argument  ;  xO  is  the  initial  state  values  ;  str  is  provided  only  for 
consistency  %  with  the  S-fimction  API  for  block  diagrams  ;  ts  is  a  two  column  matrix  containing 
the  sample  times  %  and  offsets  of  states  associated  with  the  block 
case  0 
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[sys,xO,str,ts]  =  mdlInitiaIizeSizes(A,B,C,D); 


%  Calculate  derivatives 

%  t  is  the  time  ;  x  is  the  state  vector ;  u  is  the  input  vector 
case  1 

sys  =  mdlDerivatives(t,x,u,A,B,C,D); 

%  Calculate  outputs 
case  3 

sys  =  mdlOutputs(t,x,u,A,B,C,D); 

%  Nothing  is  defined  for  calculation  of  next  sample  hit,  updating  discrete  states  and  ending  the 
%  simulation  tasks 
case  {2,4,9} 
sys  =  D; 


otherwise 

eiTor(['Unhandled  flag  -  ,num2str(flag)]); 
end 

%  enddenemel 


%  - -  '  ^ . . . . 

%  mdllnitializeSizes 

%  Return  the  sizes,  initial  conditions,  and  sample  times  for  the  S-fimction 

function  [sys,xO,str,ts]  =  mdlInitializeSizes(A,B,C,D) 

%  Call  simsizes  for  a  sizes  structure,  fill  it  in,  and  convert  it  to  a  sizes  array 
sizes  =  simsizes; 

%  Number  of  continuous  states 
sizes.NumContStates  =  7; 
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%  Number  of  dicrete  states 
sizes.NumDiscStates  =  0; 

%  Number  of  outputs 
sizes.NumOutputs  =  7; 

%  Number  of  inputs 
sizes-Numlnputs  =  5; 

%  Flag  for  direct  feedthrough 
sizes.DirFeedthrough=  0; 

%  Number  of  sample  times 
sizes.NumSampleTimes=  1; 

%  TTie  following  statement  passes  the  information  in  the  sizes  structure  to  sys,  a  vector  that  holds 
the  %  information  for  use  by  simulink 
sys  =  simsizes(sizes); 

%  Initialize  the  initial  conditions 
xO  =  [0000000]; 

%  str  is  an  empty  matrix 

str=n; 


%  Because  the  system  is  continuous,  ts  and  its  offset  becomes  0. 
ts  =[0  0]; 

%  end  mdllnitializeSizes 


%  . —  . .  . —  '  ' 

%  mdlDerivatives 

%  Return  the  sizes,  initial  conditions,  and  sample  times  for  the  S-fiuiction 

%  . - . :^=====^ 
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function  sys  =  mdlDerivatives(t,x,u,A,B,C,D) 
global  riio  Zqdot  Zwdot  Mqdot  Mwdot  Wo  g  lyo  L 


%  Plane  angle 
dr  =  u(3); 

%  Current 
Ucz  =  u(5); 

%  Velocity 
V  =  u(4); 

%  State  Variables 
theta  =  x(l); 
w  =  x(2); 
q  =  x(3); 
z  =  x(4); 

Xs  =  x(5); 
dl  =  x(6); 
d2  =  x(7); 

Bu=A(2);  ZG=A(4);  xG=A(9);  Zw=A(10); 

Mw=A(ll);  xB=A(16);  Zq=A(17);  Mq=A(18);  ZB=A(23); 

Zd=B(16);  Md=B(17); 

L1=B(3);  L2=B(10); 

%  '  ''  . - :  ...z 

%  This  part  will  be  used  only  when  the  system  is  simulated  with  thrusters. 
Zheave  =  0;Mthrust  =  0; 
if  abs(u(l))  >  0; 

Zheave  =  2;Mthrust  =  38.63; 
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%  Weight,  mass  ,  and  moment  of  inertia 
W  =  Wo  +  dl+d2; 
m  =  W/g; 

ly  =  lyo  +  dl/g*(Lr2)  +  d2/g*(L2^2); 

%MNfetrix 

Mo  =  [1  0  0  0  0  0  0;0  (m-Zwdot)  (-m*xG-Zqdot)  0  0  0  0;0  (-m*xG-Mwdot)  (ly-Mqdot)  0  0  0  0; 

0  0  0  1  0  0  0;0  0  0  0  1  0  0;0  0  0  0  0  1  0;0  0  0  0  0  0  1]; 

%  . . 

%  This  function  calculates  drag  force  and  drag  moment 
[Mdrag,Zdrag]  =  Mdtag(x); 

%  — 

%  Equations  of  motion  v  represents  forw.spd.  since  u  is  used  for  pump  flowrate 
fl=q; 

f2  =  (m+Zq)*v*q  +  m*ZG*(q'^2)  +  Zw*v*w  +  (W-Bu)*cos(theta)  +(v^2)*Zd*dr  -  Zdrag  + 
Zheave; 

f3  =  (Mq-m*xG)*v*q  -  m*ZG*w*q  +  Mw*v*w  -  (xG*Wo-xB*Bu)*cos(theta)  ... 

-  (ZG*W-ZB*Bu)*sin(theta)  +  (v^2)*Md*dr  +(-Ll*dl  +  L2*d2)*cos(theta)  -  Mdrag- 
Mthrust*q; 

f4  =  -v*sui(theta)  +  w*cos(theta); 
f5  =  v*cos(theta)  +  w*sin(theta)  +  Ucz; 
f6=u(l); 
f7=u(2); 

sys  =  inv(Mo)*[fl;f2;f3;f4;f5;f6;f7]; 

%  end  mdlDerivatives 


%  -  - - -  . . . 

%  mdlOutputs 

%  Return  the  derivatives  for  the  continuous  states 

%  '  . .  . = 

function  sys  =  mdlOutputs(t,x,u,A,B,C,D) 
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sys  =  C*x  +  D*u; 
%  end  mdlOu^uts 


%  .  . . 

%Mdrag 

%  Makes  calculations  for  drag  terms  in  the  equations  of  motion 

%  . . —  . . .  . . ^ 

function  [Mdrag,Zdrag]  =  Mdrag(x); 

global  rho  Zqdot  Zwdot  Mqdot  Mwdot  Wo  g  lyo  L 

CD  =  1.2; 

%  Beam  and  length  definitions  for  trapaziodal  rule 
xL=[0.0,-43.9/12.0,-39.2/12.0,-35.2/12.0,-31. 2/12.0,-27.2/12.0,- 
0.0/12.0,0.0/12.0,10.0/12.0,26.8/12.0, 
32.0/12.0,37.8/12.0,40.8/12.0,42.3/12.0,43.3/12.0,43.7/12.0]; 
xl  =  xL  +  L/2; 

bm=[0.0,16.5/12.0,16.5/12.0,16.5/12.0,16.5/12.0,16.5/12.0,16.5/12.0,16.5/12.0,16.5/12.0,16.5/ 

12.0,15.5/12.0,12.4/12.0,9.5/12.0,7.0/12.0,4.0/12.0,0.0/12.0]; 

X2  =  x(2); 

X3=x(3); 

ifX2  =  0, 

X2  =  le-5; 
end; 

ifX3  =  0, 

X3  =  le-5; 
end; 

Zval  =  bm.*((X2-xL.*X3).^3)./(abs(X2-xL.*X3)); 

Mval  =  bm.*(((X2-xL.*X3).^3).*xL)./(abs(X2-xL.*X3)); 
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ZDragval=0; 

MDragval=0; 

%  Trapaziodal  integration 
for  n=l:length(xL)-l, 

ZdragvaI=0.5*(Zval(n)4-Zval(n+l))*(xL(n+l)-xL(n)); 

Mdragval=0.5*(MvaI(n)+Mval(n+l))*(xL(n+l)-xL(n)); 

ZDragval=ZDragval+Zdragval; 

MDragval=MDragval+MdragvaI; 

end; 

Zdrag=(0.5)*rho*CD*ZDragval; 

Mdrag=(0.5)*rho*CD*MDragval; 

%  end  Mdrag 


%  -  .  ■—  - 

%  smcont 

%  Makes  calculations  for  flight  controller  using  sliding  mode  techniques 

function  plane_ang  =  smcont(u) 

%  State  variable  ;  theta,  w,  q,  z 
x  =  u(l:4); 

%  Commanded  flight  depth 
zcom  =  u(5); 

%  Forward  speed 
v  =  4; 

%  Matrices  of  linear  equations  of  motion 

Mo  =  [1  00  0;0  (m-Zwdot)  (-m*xG-Zqdot)  0;0  (-m*xG-Mwdot)  (ly-Mqdot)  0;0  0  0  1]; 
Ao=[0  0  1  0;0  Zw*v  (m+Zq)*v  0;-(ZG*W-ZB*Bu)  Mw*v  (Mq-m*xG)*v  0;-v  1  0  0]; 
Bo=[0;Zd*(v''2);Md*(v^2);0]; 


A  =  inv(Mo)*Ao; 
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B  =  inv(Mo)*Bo; 


%  Pole  placement 

pp  =  [-2,-2. 1, -2.2,0]; 

kk  =  place(A,B,pp); 
Ac  =  A-B*kk; 

[mm,nn]  =  eig(Ac'); 
ss  =  mm(:,4); 
xcom  =  [0;0;0;zcom]; 
sig  =  ss'*(x-xcom); 


phi  =0.1; 

Nmax  =  2; 

eta  =  Nmax*0.4/inv((ss'*B)); 

%  Control  law 

plane_ang  =  -kk*x-Nniax*0.4*sign(inv((ss'*B)))*tanh((sig/phi)); 
%  end  smcont 


%  -  '  '  ■  . -  . . 

%lq 

%  Calculates  gains  for  weight  controller  using  LQR  techniques 

%  .  . . 

%  Forward  speed 
v  =  0.1; 


%  Matrices  of  linear  equations  of  motion 

Mo  =  [1  0  0  0  0  0;0  (m-Zwdot)  (-m*xG-Zqdot)  0  0  0;0  (-m*xG-Mwdot)  (ly-Mqdot)  0  0  0; 

0  0  0  1  0  0;0  0  0  0  1  0;0  0  0  0  0  1]; 

Ao=[0,0,l,0,0,0;0,Zw*v,(m+Zq)*v,0,l,l;-(ZG*W-ZB*Bu),Mw*v,(Mq-m*xG)*v,0,-Ll,L2; 
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-v,l,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0]; 

Bo=[0,0;0,0;0,0;0,0;l,0;0,l]; 


A  =  inv(Mo)*Ao; 
B  =  inv(Mo)*Bo; 


Q  =  eye(6); 

Q(l)=10000; 

Q(22)=100; 

R  =  300000*eye(2); 

[k,s,m]=lqr(A,B,Q,R); 

k 

%  end  Iqr 


% 

%  weightcont 

%  Calculates  the  flow  rates  for  each  ballast  pump 
% 

function  wcur  =  weightcont(u) 

%  state  variables 
x=u(l:6); 

%  forward  speed 
v  =  u(7); 

%  ground  depth 
zgr=  10; 


xc  =  [0  0  0  zgr  0  0]'; 
xerr  =  x  -  xc; 
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%  Matrices  for  linear  equations  of  motion 

Mo  =  [1  0  0  0  0  0;0  (m-Zwdot)  (-m*xG-Zqdot)  0  0  0;0  (-m*xG-Mwdot)  (ly-Mqdot)  0  0  0; 
0  0  0  1  0  0;0  0  0  0  1  0;0  0  0  0  0  1]; 


Ao  =  [0,0,1, 0A0;0,Zw*v,(m+Zq)*v,0,l,l;-(ZG*W-ZB*Bu),Mw*v,(Mq-m*xG)*v,0,-Ll,L2; 
-v,l,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0]; 

Bo=[0,0;0,0;0,0;0,0;l,0;0,l]; 


A  =  inv(Mo)*Ao; 
B  =  inv(Mo)*Bo; 


%  Checks  if  the  grounding  procedure  started 
ifv>0.1; 

wcur=  [0;0]; 
else 

%  Gains  found  in  Iq  function 

Kl  =  [0.0608  0.1263  -0.0526  0.0138  0.0754  0.0365]; 

K2  =  [-0.0322  0.1294  0.0021  0.0120  0.0365  0.0613]; 

%  Flow  rate  for  pump-1 
wcur(l)  =  -Kl*xerr; 

%  If  there  is  no  water  in  ballast  tank-1  and  flow  rate  for  pump-1  is  negative,  then  turns  off  the 
pump 

if(u(5)<=0&wcur(l)<0); 

wcur(l)  =  0; 
end; 

%  Flow  rate  for  piunp-2 
wcur(2)  =  -K2*xerr; 

%  If  there  is  no  water  in  ballast  tank-2  and  flow  rate  for  pump-2  is  negative,  then  turns  off  the 
pump 
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if  (u(6)  <=  0  &  wcur(2)  <  0); 

wcur(2)  =  0; 
end; 

wcur  =  [wcur(l);wcur(2)]; 
end; 

%  end  weightcont 

%  --  - 

%  speedcont 

%  Controls  the  forward  speed 

%  .  . 

function  vcom  =  speedcont(u) 

%  Current  velocity 
vcur  =  u(l); 

%  Current  depth 
zcurrent  =  u(2); 

%  Commanded  depth  of  flight 
zcom  =  u(3); 
z  =  zcom  -  zcurrent; 

%  Current  longitudinal  location 
xreal  =  u(5); 

%  Commanded  longitudinal  location 
xcomm  =  u(6); 

%  Bottom  depth 

zgmd  =  u(7); 

location  =  xcomm  -  xreal; 

%  deceleration 
acc  =  0.625; 

%  Forward  speed  during  the  flight 
Vo  =  u(4); 
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%  Time 
t  =  Vo/acc; 

xstop  =  'Vo*t-0.5*acc*(t^2); 
zvert  =  2gmd  -  zcom; 
tvert  =  zvert/0.11; 
xstop2  =  0.6*tvert  +  9; 

%  Distance  needed  to  complete  stop 
xstop  1  =  xstop  +  xstop2; 

%  Checks  if  the  vehicle  is  in  the  commanded  depdi  and  reduces  to  speed 
if  abs(z)  <  0.05; 
if  location  <  xstop  1; 

vcen  =  abs(u(l)  -  0.1)  ; 
ifvcen>  0.1; 

vcom  =  vcen; 
else 

vcom  =  0.1; 

end; 

else 

vcom  =  u(4); 

end; 

else 

vcom  =  u(4); 

end; 

%  end  speedcont 


%  - 

%  seccont 

%  Keeps  the  vehicle’s  velocity  at  0. 1  fl/sec  during  grounding 

%  . —  -  - 

function  vsec  =  seccont(u) 

%  Difference  between  bottom  and  current  depth 
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gmd  =  u(4)  -  u(5); 


if(u(l)>0|u(3)>0); 

vsec  =  0.1; 
elseif  gmd  >  3; 

vsec  =  0.1; 
else 

vsec  =  u(2); 
end; 

%  end  seccont 


% 

%  planeangcont 

%  Keeps  the  plane  angles  at  0  degree  during  grounding 

%  .  . -  . . 

function  ang  =  planeangcont(u) 
ifu(l)>0.1; 

ang  =  u(2); 
else 

ang  =  0; 
end; 

%  end  planeangcont 

%  .  — 

%  grounddepth 

%  Stops  the  simulation  at  the  ground  depth  defined  by  the  user 

%  .  --  - 

function  gmd  =  grounddepth(u) 

%  Bootom  depth 
ground  =  u(l); 

%  Ciurent  depth 
actdpth  =  u(2); 

if  actdpth  >  ground; 
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gmd  =  ground; 
else 

gmd  =  actdpth; 
end; 

%  end  grounddepth 
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APPENDIX  B 


The  parameter  values  used  to  simulate  the  dynamic  behavior  of  the  NPS  Phoenix  are 


given  in  this  appendix  : 

A.  PHYSICAL  PARAMETERS 

W  Vehicle  Weight  =  435  lbs 

B  Vehicle  Buoyancy  =  435  lbs 

L  Characteristic  Length  =  94  in. 

Li  Bow  Ballast  Tank  Offset  from  C.G.  =  24.25  in. 

L2  Stem  Ballast  Tank  Offset  from  C.G.  =  14.75  in. 

lyz  Moment  of  Inertia  =  42  Ib-ft-sec^ 


Xg  X  Coordinate  of  C.G.  From  Body-Fixed  Origin  =  0.0  in 

xb  X  Coordinate  of  C.B.  From  Body-Fixed  Origin  =  0.0  in 

Zb  z  Coordinate  of  C.B.  From  Body-Fixed  Origin  =  0.0  in 

zq  z  Coordinate  of  C.G.  From  Body-Fixed  Origin  =  0.5  in 

B.  CONTROL  INPUTS 

6b  Bow  Plane  Deflection 

6s  Stem  Plane  Deflection 

fi  Bow  BaUast  Pump  Flow  Rate 

fz  Stem  Ballast  Pump  Flow  Rate 

C.  NON-DIMENSIONALIZED  HYDRODYNAMIC  COEFFICIENTS 


=  -0.00253 

z'. 

=  -0.09340 

z; 

=  -0.07013 

K 

=  -0.78440 

7' 

^6s 

=  -0.02110 

zk 

=  -0.02110 

K 

=  -0.00625 

m; 

=  -0.00253 

m; 

=  -0.01530 

=  0.05122 

=  -1.7664 

ML 

=  1.3260 
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